#include "FOC_run.h"
#include "ThreeHall.h"
//#include "adc.h"
//#include "tim.h"
#include "Axis_transform.h"
#include "Svpwm_dq.h"
#include "Task_function.h"
#include "PI_Cale.h"
#include "Timer.h"
#include "RS485.h"
#include "Over_Modulation.h"
#include "main_user.h"
//#include "SEGGER_RTT_Port.h"

#define DEBUG 1
#if DEBUG

#else

#endif



#define Q_mode 0  //定点数格式模式

extern  PI_Control   pi_spd ;
extern  PI_Control   pi_id  ;
extern  PI_Control   pi_iq  ;
//extern   ADCSamp    ADCSampPare;
//extern   Hall       Hall_Three;
extern   logic      logicContr;
extern   TaskTime   TaskTimePare;  
extern   CLARKE     ClarkeI;
extern   PARK       ParkI;
extern   IPARK      IparkU;
extern   SVPWM      Svpwmdq;
extern   IQSin_Cos  AngleSin_Cos;
//extern   Test       TestPare;
extern   IQAtan     IQAtan_Pare;

Motor_Params_t Motor_Params = Motor_Params_DEFAULTS;

uint16_t  FilK1=328;
uint16_t  FilK2=696;

volatile uint16_t   PWM_DUTY[3];
extern uint32_t adc1_in0, adc1_in1, adc2_in2, adc1_in8;

uint32_t start,end,cycles;
float time_us;
void (* func_ptr)(ADC_HandleTypeDef *hadc) = NULL;
// 10khz  ADC注入组采样完成中断
uint32_t Predict_ThreeHallangle;
void HAL_ADCEx_InjectedConvCpltCallback( ADC_HandleTypeDef *hadc)
{
	extern ADC_HandleTypeDef hadc1, hadc2;
	extern TIM_HandleTypeDef htim1, htim4, htim3;


	if( hadc->Instance == hadc1.Instance)
	{
		// start timer to calculate the execution time of the function. 单位1/8us
		//__HAL_TIM_SET_COUNTER( &htim3 , 0);

	
#if DEBUG	
		
		start = DWT->CYCCNT; // 记录起始时间
#else
										
#endif
		
		Predict_ThreeHallangle = ThreeHallanglecale();  
		//uint32_t Predict_eleangle = Predict_ThreeHallangle;
		/*********IF VF强拖判断同步电角度*********/
		//uint32_t Predict_eleangle = IF_id_RUN(1, 1000);
		uint32_t Predict_eleangle = IF_RUN(600, 500, 30000);										//计算HALL电角度和机械角度
	  //uint16_t Predict_eleangle = VF_RUN(400, 10*32768, 60000);
		//Hall_Three.Hall_State = GET_HALLState_debounce_dynamic();
		
		mc_info.encoder_theta = (uint16_t)((12868 * Predict_eleangle)>> 14);
		/*********************  Processing EventTask   *************************/
        TaskTimePare.PWMZD_count++;
		 if(TaskTimePare.PWMZD_count % Hall_Three.Speed_Calculate_Times == 0 ) 					// 1ms执行一次
		 {
				
				Hall_Three_Speedcale( );  							//计算速度
																																																																																																																																																																																																																																																																																	
				knob_control( );   											//检测电位器

			 /****** 4、 开始计算速度 PI环路，速度环执行频率一般小于速度更新频率数倍******/
			 if(TaskTimePare.PWMZD_count >= Hall_Three.Speed_Calculate_Times*2) 					// 2ms执行一次
		   {
				  TaskTimePare.PWMZD_count=0;
					pi_spd.Fbk = Hall_Three.Speed_RPM ;  			
					PI_Controller_conditional((p_PI_Control)&pi_spd); 		// 计算PI值，这里的速度环输出pi_spd.OutF作为电流环iq的输入
					
				 //对计算出的速度值进行一阶低通滤波，这里K1K2为Q10格式，因此计算后的pi_spd.OutF还是真实值
					pi_spd.OutF= _IQ10mpy(HallK1_RPM,pi_spd.OutF)+_IQ10mpy(HallK2_RPM,pi_spd.Out); 
			 }
		 }

		/***********************   Processing ADC  ***************************/
		
	  ADC_Sample();
    
		mc_info.isens_b = ADCSampPare.PhaseV_CurrFitter;
		mc_info.isens_a = ADCSampPare.PhaseU_CurrFitter;   
		mc_info.isens_c = -( mc_info.isens_b + mc_info.isens_a);
		 

		/**************************   ADC END   ******************************/
    // 6us.

		/***********************   FOC Algorithm   ***************************/
		 
		/****** 1、 CLARKE变换******/
		ClarkeI.As = ADCSampPare.PhaseU_CurrFitter;   //采样的相电流滤波值
	  ClarkeI.Bs = ADCSampPare.PhaseV_CurrFitter;   
	  CLARKE_Cale((p_CLARKE)&ClarkeI ); 					 //输出CLARKE变换值的定点数格式和CLARKE输入值一致，即和相电流定点数相同

    /****** 2、 park变换******/
	  ParkI.Alpha=ClarkeI.Alpha;  
	  ParkI.Beta=ClarkeI.Beta;
	  AngleSin_Cos.IQAngle= Predict_eleangle;  //从0-2pi映射到0-65535
	  IQSin_Cos_Cale((p_IQSin_Cos)&AngleSin_Cos); //将当前计算的IQ电角度对应的正余弦值用查表法找出最接近的Q15格式定点数sin、cos值
		ParkI.Sine = AngleSin_Cos.IQSin;      
	  ParkI.Cosine = AngleSin_Cos.IQCos;   
	  PARK_Cale((p_PARK)&ParkI);  							 //输出PARK变换值的定点数格式和PARK输入值一致，即和相电流定点数相同
	
	  /****** 3、 给定PI控制目标值******/
		if(logicContr.Run_mode==5)  //只有电流内环
    {
			pi_id.Ref = pi_id.Ref;
			pi_iq.Ref = pi_iq.Ref;//pi_iq.Ref;
		}
	  else   //有速度外环
		{
			pi_id.Ref = 0;              //表贴式电机，将目标电流Id设置为0
	    pi_iq.Ref = pi_spd.OutF;    //速度环输出pi_spd.OutF作为电流环iq的输入pi_iq.Ref 
		}

    /****** 4、 开始计算电流 PI环路  输出为为Q15定点数  
		            输出限幅：防止Ud、Ud单个电压大于FOC矢量圆的最大半径（等幅值变换下为Udc/√3）******/
		
    pi_id.Fbk = ParkI.Ds;      //PI控制的返回值为park变换后的值，这里PI输入值定点数格式和相电流定点数相同
	  PI_Controller_conditional((p_PI_Control)&pi_id);//PI_Controller((p_PI_Control)&pi_id);  			// id进行PI计算
		//将PI控制的输出值进行一次低通滤波，这里K1K2为Q10格式，因此计算后的pi_id.OutF定点数格式不变，为Q15定点数
    pi_id.OutF= _IQ10mpy(ADCSampPare.Coeff_filterK1,pi_id.OutF)+_IQ10mpy(ADCSampPare.Coeff_filterK2,pi_id.Out);		

	  pi_iq.Fbk = ParkI.Qs;      //PI控制的返回值为park变换后的值
	  PI_Controller_conditional((p_PI_Control)&pi_iq);//PI_Controller((p_PI_Control)&pi_iq);  			// iq进行PI计算
		//将PI控制的输出值进行一次低通滤波
    pi_iq.OutF= _IQ10mpy(ADCSampPare.Coeff_filterK1,pi_iq.OutF)+_IQ10mpy(ADCSampPare.Coeff_filterK2,pi_iq.Out);	
   
		//pi_id.OutF <= _IQ15(Motor_Params.VBUS) *577/1000;   √3/3≈577/1000  这里把输出限幅放到PI控制器中
		//pi_iq.OutF <= _IQ15(Motor_Params.VBUS) *577/1000;
		
	
	  /****** 5、 根据运行模式给出反park变换的输入值（要使用的PI控制结果）******/
	  if(logicContr.Run_mode==1)  // 只有外层速度环开环模式
	  {
	    IparkU.Ds=0;   														
	    IparkU.Qs= 10*pi_spd.Ref;  
	  }
		//闭环控制模式，输出Park变换后进行PI控制的值pi_iq.OutF
	  if(logicContr.Run_mode==2)   // 只有外层速度环，不经过iq内层电流环，正转模式
	  {
			IparkU.Ds= pi_id.OutF;   									// id目标值为0
			IparkU.Qs= pi_spd.OutF;  														 
	  }
    if(logicContr.Run_mode==3)   //只有外层速度环，不经过iq内层电流环，反转模式
	  {  							
			IparkU.Ds= pi_id.OutF;                   // id目标值为0
      IparkU.Qs= -pi_spd.OutF;  								
    }
    if(logicContr.Run_mode==4)  //正常FOC模式速度外环电流内环
    {
			 IparkU.Ds= pi_id.OutF;   								// 计算出的内层电流环输出作为逆Park变换输入
			 IparkU.Qs= pi_iq.OutF;   								// 1=2^15=32768 
    }
		if(logicContr.Run_mode==5)  //只有电流内环
    {
			 IparkU.Ds= pi_id.OutF;   								// 计算出的内层电流环输出作为逆Park变换输入
			 IparkU.Qs= pi_iq.OutF;   								// 1=2^15=32768 
    }

    //此时为Q15定点数，真实值范围为-1到1    
		IparkU.Sine=AngleSin_Cos.IQSin;
		IparkU.Cosine=AngleSin_Cos.IQCos;
		IPARK_Cale((p_IPARK)&IparkU);      					 //输出IPARK变换值的定点数格式和IPARK输入值一致，为Q15定点数，数值为电压值*Q15

		
	  /****** TBD:6、 防止过调制  即防止Ud、Ud的合成矢量大于FOC矢量圆的最大半径（等幅值变换下为Udc/√3） ******/
		RevPark_Circle_Limitation(&IparkU.Alpha, &IparkU.Alpha);

		/****** 7、 电压归一化  ******/
	  //从SVPWM可以看出应该为扇区+时长法，输出值为-1到1的Q15定点数，根据其中的计算公式输入值应该也在-32767到32767之间
		//由于过调制之后的电压为-Udc/√3到Udc/√3，Q15定点数就是-Udc/√3*32767到Udc/√3*32767，这里就肯定超出规定输入范围了，所以需要做一个电压归一化
	  //至于归一化系数要为多少，1/(Udc/√3)?感觉可能还是会超范围，那就用更小的1/(Udc/2/3)?这里暂时不知道系数为多少，但肯定要归一化
	  IparkU.Alpha =  IparkU.Alpha / Motor_Params.VBUS *3/2;        
		IparkU.Beta =  IparkU.Beta / Motor_Params.VBUS *3/2;
		
		/****** 8、 输出SVPWM******/
		Svpwmdq.Ualpha = IparkU.Alpha;    
		Svpwmdq.Ubeta = IparkU.Beta;
		SVPWM_Cale((p_SVPWM)&Svpwmdq);  						//SVPWM计算占空比   输出TaTbTc的定点数格式和SVPWM输入值一致，为Q15定点数
		Svpwm_Outpwm( );  													// PWM输出
		
		/*********************   FOC Algorithm END ***************************/
		

		// Upload debug parameters to simulink.
//	 if(TaskTimePare.PWMZD_count==2 ) 
//		  UploadData(&mc_info);		// 7us.
		
	
#if DEBUG	
    end = DWT->CYCCNT;   // 记录结束时间
		
    cycles  = end - start; // 计算运行的 CPU 周期数
		// 处理溢出情况（如果测量过程中发生了溢出）
    if (end < start) {
				cycles = (0x100000000ULL - start) + end;  // 处理溢出
		} else {
			cycles = end - start;
		}
		time_us  = (float)cycles / (SystemCoreClock / 1e6); // 转换为微秒
		func_ptr = HAL_ADCEx_InjectedConvCpltCallback;
#else
									
#endif		
		
		//SEGGER_RTT_Port_Write();
		//mc_info.run_realtime = __HAL_TIM_GET_COUNTER( &htim3);

	}
}


void ParaInit(void)
{
	mc_info.refIq = 0;
	mc_info	.refRPM = 0;
	mc_info.mc_state = MC_RDY;
	mc_info.mc_err = NONE_ERR;
}


void Svpwm_Outpwm(void)
{
	 /*这里用Q15定点数乘法，而PWM_HalfPerMax不为定点数，
	 所以这里其实是将Q15定点数Svpwmdq.Ta还原成-1到1的真实值然后在计算PWM占空比*/

	 PWM_DUTY[0]= _IQmpy(PWM_HalfPerMax,Svpwmdq.Ta)+ PWM_HalfPerMax;  
	 PWM_DUTY[1]= _IQmpy(PWM_HalfPerMax,Svpwmdq.Tb)+ PWM_HalfPerMax;
	 PWM_DUTY[2]= _IQmpy(PWM_HalfPerMax,Svpwmdq.Tc)+ PWM_HalfPerMax;


	 TIM1->CCR1 = PWM_DUTY[0];//PWM_DUTY[0]  (EQEPPare.JZElecTheta>>5)
   TIM1->CCR2 = PWM_DUTY[1];//PWM_DUTY[1]
   TIM1->CCR3 = PWM_DUTY[2] ;//PWM_DUTY[2]
}

//IF强拖启动  通过电流环，直接给定一个id和较小的固定角速度，待电机匀速转动后HALL状态和电角度一一对应
uint32_t IF_id_RUN(uint16_t eleangle_step, uint16_t Id_ref)
{
	static uint32_t Angular_Angle = 0;
    Angular_Angle += eleangle_step;  //计算本次电角速度
	
    if(Angular_Angle >= 65536)
        Angular_Angle -= 65536;

	pi_iq.Ref = 0;
	pi_id.Ref = Id_ref;
	return  Angular_Angle;  //返回Q16格式的电角度
}

//IF爬坡强拖启动  通过电流环，直接给定一个iq然后匀速增加电角度
uint32_t IF_RUN(uint16_t RPM_ref, uint16_t Iq_ref, uint16_t Time_ms)
{
	  static float Angular_Speed;
	  static float Angular_Angle = 0;
	
    uint64_t Angular_Speed_ref = (uint64_t)(RPM_ref)*65536ULL/60ULL*(uint64_t)Hall_Three.Poles; 	//计算目标电角速度  Q16格式的 rad/s
    uint32_t IF_FOC_num = (uint32_t)Time_ms*1000/Hall_Three.Speed_PWM_Time;   //爬坡过程FOC中断次数
	  uint32_t Angular_Speed_step = Angular_Speed_ref / IF_FOC_num;  //每次FOC中断的爬坡电角速度步进值

    Angular_Speed += Angular_Speed_step;  //计算本次电角速度
	  if( Angular_Speed >= Angular_Speed_ref)
			Angular_Speed = Angular_Speed_ref;
			
    Angular_Angle = Angular_Angle + Angular_Speed * (float)Hall_Three.Speed_PWM_Time / 1000000;  //计算本次电角度
    if(Angular_Angle >= 65536)
        Angular_Angle -= 65536;
    uint32_t Angular_Angle_Q16 = (uint32_t)(Angular_Angle);  //将电角度转换为Q15格式

	pi_iq.Ref = Iq_ref;
	pi_id.Ref = 0;
	return Angular_Angle_Q16;  //返回Q16格式的电角度
}

//VF爬坡强拖启动  跳过电流环、速度环、位置环，直接给定目标Ud = 0，Uq为一个值，然后匀速增加电角度
float Angular_Speed = 0 , Angular_Angle = 0;
uint32_t Angular_Angle_Q16 = 0;
uint64_t Angular_Speed_ref;
uint32_t IF_FOC_num;
uint32_t Angular_Speed_step;

uint32_t VF_RUN(uint16_t RPM_ref, uint32_t Uq_ref, uint16_t Time_ms)
{
	
	  Angular_Speed_ref = (uint64_t)(RPM_ref)*65536ULL/60ULL*(uint64_t)Hall_Three.Poles; 	//计算目标电角速度  0-360度换算为0-65535
    IF_FOC_num = (uint32_t)Time_ms*1000/Hall_Three.Speed_PWM_Time;   //爬坡过程FOC中断次数
	  Angular_Speed_step =Angular_Speed_ref / IF_FOC_num;  //每次FOC中断的爬坡电角速度步进值

    Angular_Speed += Angular_Speed_step;  //计算本次电角速度
	  if( Angular_Speed >= Angular_Speed_ref)
			Angular_Speed = Angular_Speed_ref;
			
    Angular_Angle = Angular_Angle + Angular_Speed * (float)Hall_Three.Speed_PWM_Time / 1000000;  //计算本次电角度
    if(Angular_Angle >= 65536)
        Angular_Angle -= 65536;
    Angular_Angle_Q16 = (uint32_t)(Angular_Angle);  //将电角度转换为Q15格式

		IparkU.Ds= 0;   									// id目标值为0
		IparkU.Qs= Uq_ref;
	return Angular_Angle_Q16;  //返回Q16格式的电角度
}


